#include "PrecompiledHeaders.h"
#include "MovingObject.h"
#include "Constants.h"
#include <stdlib.h> 
#include "StringTokenizer.h"
#include "FromString.h"
#include "Clone.h"

/*
 * Default constructor
 */
MovingObject::MovingObject()
{
    body = NULL;
    ogre_node = NULL;
    up_joint = NULL;
    movement_joint = next_movement_joint = NULL;
}

/*
 * Constructor to create moving object from the dotSceneLoader
 */
MovingObject::MovingObject( OgreNewt::Body* b, 
                            std::map<Ogre::String, Ogre::String> *childNodeIDs)
{
    body = b;
    
    body->setUserData(this);
    
    body->getMassMatrix(original_mass,original_inertia);
    body->getPositionOrientation(original_position, original_orientation);
            
    ogre_node = dynamic_cast<Ogre::SceneNode*>(body->getOgreNode());
    if (!ogre_node) throw new Ogre::Exception(80002, "Invalid node for moving object - must be scene node", "MovingObject::MovingObject");
        
    ogre_node->setInitialState();
        
    Ogre::SceneManager *scene_manager = ogre_node->getCreator();
    
    Ogre::NameValuePairList pair_list;
    pair_list["numberOfChains"] = "1";
    pair_list["maxElements"] = "80";
    Ogre::RibbonTrail *trail = (Ogre::RibbonTrail*) scene_manager->createMovableObject(ogre_node->getName() + "_tr",
                                                                                       "RibbonTrail", &pair_list);
    trail->setMaterialName("Particles/Dust");
    trail->setTrailLength(400);
    scene_manager->getRootSceneNode()->createChildSceneNode()->attachObject(trail);
    trail->setInitialColour(0, 1, 0.8, 0);
    trail->setColourChange(0, 0.5, 0.5, 0.5, 0.5);
    trail->setInitialWidth(0, 5);
    trail->addNode(ogre_node);                            
    
    
    
    Ogre::Node::ChildNodeIterator children = ogre_node->getChildIterator();
    
    std::vector<PathNode>::iterator main_it;
    
    std::vector<int> indices;
    std::vector<int>::iterator i_it;
    
    while( children.hasMoreElements() )
    {
        Ogre::Node *child = children.getNext();
        Ogre::String name = child->getName();
        
        size_t i = name.find("path_");
        if (i != Ogre::String::npos)
        {
            PathNode pn = createPathNode(name, childNodeIDs, child->_getDerivedPosition());
            size_t ind_start = i + 5;
            size_t ind_end = name.length() - 4;
            
            int index = atoi(name.substr(ind_start, ind_end - ind_start).c_str());
            i_it = indices.begin();
            main_it = path_nodes.begin();
            
            while(i_it < indices.end()) {
                if (*i_it > index) break;
                i_it ++; main_it++;
            }
            path_nodes.insert(main_it, pn);
            indices.insert(i_it, index);
        }
    }
    
    ogre_node->removeAndDestroyAllChildren(); 
        
    
    Ogre::String params = Ogre::any_cast<Ogre::String>(ogre_node->getAttachedObject(0)->getUserAny());
    StringTokenizer str_tok (params, " ,");
    
    default_speed = 10; default_delay = 0;
    initial_wait = false;
    while(str_tok.hasMoreTokens())
    {
        Ogre::String param = str_tok.getNext();
        if (param.substr(0,5) == "speed")
            from_string<Ogre::Real>( default_speed, param.substr(6).c_str(), std::dec );
        else if (param.substr(0,5) == "delay")
            from_string<Ogre::Real>( default_delay, param.substr(6).c_str(), std::dec );
        else if (param.substr(0,13) == "wait_for_ball")
            initial_wait = true;
    }
    
    body->setAutoFreeze(0);
    up_joint = new OgreNewt::BasicJoints::UpVector(body->getWorld(), body, Ogre::Vector3::UNIT_Y);
        
    movement_joint = NULL;
    
    init();
}

MovingObject::~MovingObject()
{
    //if (movement_joint) movement_joint->setCallback(NULL);
    //if (next_movement_joint) next_movement_joint->setCallback(NULL);
    //delete up_joint;
    //delete movement_joint;
}

void MovingObject::setMaterialID(OgreNewt::MaterialID *mat_id)
{
    body->setMaterialGroupID(mat_id);
}

void MovingObject::init()
{
    current_target = 0;
    target_speed = default_speed;
    current_delay = default_delay;
    state = WAITING;
    current_speed = wait_duration = 0;
    waiting_for_ball = initial_wait;
    deceleration_distance = 0;
           
    current_direction = path_nodes[0].position - ogre_node->getPosition();
    target_distance = current_direction.normalise();

    if (movement_joint != NULL)
    {
        movement_joint->setCallback(NULL);
        delete movement_joint;
        movement_joint = NULL;
    }
    next_movement_joint = createMovementJoint();
}

OgreNewt::BasicJoints::Slider *MovingObject::createMovementJoint()
{
    OgreNewt::BasicJoints::Slider *joint = new OgreNewt::BasicJoints::Slider(body->getWorld(), body, NULL, ogre_node->getPosition(), current_direction);
    joint->setUserData(this);
    joint->setCallback(&MovingObject::sliderCallback);
    
    joint->setStiffness(0);
    joint->setCollisionState(0);
    return joint;
}

void MovingObject::reset()
{
    if (movement_joint) {
        movement_joint->setCallback(NULL);
        delete movement_joint;
        movement_joint = NULL;
    }
    if (next_movement_joint) {
        next_movement_joint->setCallback(NULL);
        delete next_movement_joint;
        next_movement_joint = NULL;
    }
    if (up_joint) {
        delete up_joint;
        up_joint = NULL;
    }
    /*
    const OgreNewt::Collision *col = body->getCollision();
    const OgreNewt::World *world = body->getWorld();
    int bt = body->getType();
    
    delete body;
    body = new OgreNewt::Body(world, col, bt);
    body->setUserData(this);
    body->setAutoFreeze(0);
    */
    body->setPositionOrientation(original_position, original_orientation);
    /*
    ogre_node->resetToInitialState();
    
    body->attachToNode(ogre_node);
    
    body->setMassMatrix(original_mass, original_inertia);
    */
    up_joint = new OgreNewt::BasicJoints::UpVector(body->getWorld(), body, Ogre::Vector3::UNIT_Y);
    
    init();
}

PathNode MovingObject::createPathNode(Ogre::String name, 
                                      std::map<Ogre::String, Ogre::String> *node_id_map, 
                                      Ogre::Vector3 position)
{
    PathNode pn;
    pn.position = position;
    pn.speed = -1;
    pn.delay = -1;
    pn.rotate = true;
    pn.wait_for_ball = false;
    if (node_id_map != NULL) 
    {
        std::map<Ogre::String, Ogre::String>::iterator it;
        it = node_id_map->find(name);
        if (it != node_id_map->end())
        {
            StringTokenizer str_tok(it->second, " ,");
            while (str_tok.hasMoreTokens()) {
                Ogre::String param = str_tok.getNext();
                if (param.substr(0,5) == "speed")
                    from_string<Ogre::Real>( pn.speed, param.substr(6).c_str(), std::dec);
                else if (param.substr(0,5) == "delay")
                    from_string<Ogre::Real>( pn.delay, param.substr(6).c_str(), std::dec);
                else if (param.substr(0,3) == "rotate")
                    from_string<bool>( pn.rotate, param.substr(4).c_str(), std::boolalpha);
                else if (param.substr(0,13) == "wait_for_ball")
                    pn.wait_for_ball = true;
            }
        }
    }
    
        
    return pn;
}
#include "PlayState.h"
void MovingObject::sliderCallback(OgreNewt::BasicJoints::Slider *slider)
{
    MovingObject *mo = (MovingObject *)slider->getUserData();
    Ogre::Real speed = slider->getJointVeloc();
    Ogre::Real pos = slider->getJointPosit();
    Ogre::Real target_dist = mo->getTargetDistance();
    Ogre::Real dist_left = target_dist - pos;
    
    Ogre::Real acceleration = 0;
    
    //fprintf(PlayState::out_file, "timestep: %f\n", slider->getCallbackTimestep());
    fprintf(PlayState::out_file, "joint velocity: %f\n", speed);
    //fprintf(PlayState::out_file, "target dist: %f\n", target_dist);
    //fprintf(PlayState::out_file, "pos: %f\n", pos);
    
    //mo->rotateUpdate(pos, dist_left);
    
    if (mo->getState() == ACCELERATING) {
    
        // if (over half way) or (reached correct speed)
        acceleration = Constants::MOV_ACC;
        if (dist_left < pos) {
            mo->setDecelerationDistance(pos);
            mo->setState(DECELERATING);
        } else if (speed > mo->getTargetSpeed()) {
            mo->setDecelerationDistance(pos);
            mo->setState(MOVING);
        }
    }
    
    if (mo->getState() == DECELERATING) {
        if (speed < 4)
            acceleration = 0;
        else
            acceleration = -(Constants::MOV_ACC / 2);
    }
    
    if (mo->getState() == MOVING) {
        acceleration = 0;
        if (dist_left < mo->getDecelerationDistance()) {
            mo->setState(DECELERATING);
            acceleration = -(Constants::MOV_ACC);
        }
    }
    
    if (dist_left < 0.001) {
        mo->atTarget();
    }
    
    //fprintf(PlayState::out_file, "acceleration: %f\n", acceleration);
    slider->setCallbackAccel(acceleration);
    
    mo->setCurrentSpeed(speed);
    
}

void MovingObject::freeze()
{
    body->setMassMatrix( 0, Ogre::Vector3::ZERO );
}

void MovingObject::unfreeze()
{
    body->setMassMatrix( original_mass, original_inertia );
    body->unFreeze();
}

void MovingObject::atTarget()
{
    Ogre::Vector3 pos;
    Ogre::Quaternion orient;
    body->setVelocity(Ogre::Vector3::ZERO);
    body->getPositionOrientation(pos, orient);
    PathNode target = path_nodes[current_target];
    
    target_speed = (target.speed < 0) ? default_speed : target.speed;
    current_delay = (target.delay < 0) ? default_delay : target.delay;
    waiting_for_ball = target.wait_for_ball;
    body->setPositionOrientation(target.position, orient);
    
    state = WAITING;
    wait_duration = 0;
    
    current_target++;
    if (current_target >= path_nodes.size()) {
        current_target = 0;
    }
    
    PathNode next_target = path_nodes[current_target];
    current_direction = next_target.position - target.position;
    target_distance = current_direction.normalise();
    
    movement_joint->setCallback(NULL);
    
    next_movement_joint = createMovementJoint();
}

/*
int MovingObject::userBegin()
{
    int type0 = m_body0->getType() % BODY_TYPE_COUNT;
    int type1 = m_body1->getType() % BODY_TYPE_COUNT;
    return ( (m_body0->getType() / BODY_TYPE_COUNT == m_body1->getType() / BODY_TYPE_COUNT)
          && (type0 == BT_BALL || type1 == BT_BALL || type0 == BT_BASIC || type1 == BT_BASIC)
           );
}

int MovingObject::userProcess()
{
    MovingObject *mo = NULL;
    int type0 = m_body0->getType() % BODY_TYPE_COUNT;
    int type1 = m_body1->getType() % BODY_TYPE_COUNT;
    if (type0 == BT_MOVING && type1 == BT_BALL)
        mo = (MovingObject*)m_body0->getUserData();
    else if (type1 == BT_MOVING && type0 == BT_BALL)
        mo = (MovingObject*)m_body1->getUserData();
    else
        return 1;
        
    if (mo->getState() == WAITING && mo->isWaitingForBall()) mo->setWaitingForBall(false);
	return 1;
}*/

void MovingObject::ballCollision()
{
    if (state == WAITING && waiting_for_ball)
        waiting_for_ball = false;
}

bool MovingObject::frameStarted(Ogre::Real time) 
{
    Ogre::Vector3 pos;
    Ogre::Quaternion orient;
    
    body->getPositionOrientation(pos,orient);
    //target = path_nodes[current_target].position; 
    //diff = target - pos;
    //Ogre::Real dist_to_target = diff.normalise();
    
    //fprintf(PlayState::out_file, "body position: %f %f %f\n", pos.x, pos.y, pos.z);
    
    
    
    if (next_movement_joint != NULL) {
        delete movement_joint;
        movement_joint = next_movement_joint;
        next_movement_joint = NULL;
        freeze();
    }
    
    if (state == WAITING)
    {
        
        wait_duration += time;
        if (wait_duration > current_delay && waiting_for_ball == false)
        {
            state++;
            unfreeze();
        }
    }
        
    return true;
}

Ogre::Real MovingObject::getTargetSpeed()
{
    return target_speed;
}

void MovingObject::setCurrentSpeed(Ogre::Real s)
{
    current_speed = s;
}

Ogre::Real MovingObject::getCurrentSpeed()
{
    return current_speed;
}


Ogre::Real MovingObject::getTargetDistance()
{
    return target_distance;
}

int MovingObject::getState()
{
    return state;
}

void MovingObject::setState(int s)
{
    state = s;
}

void MovingObject::setDecelerationDistance(Ogre::Real d)
{
    deceleration_distance = d;
}

Ogre::Real MovingObject::getDecelerationDistance()
{
    return deceleration_distance;
}

bool MovingObject::isWaitingForBall()
{
    return waiting_for_ball;
}

void MovingObject::setWaitingForBall(bool w)
{
    waiting_for_ball = w;
}

MovingObject *MovingObject::clone(Ogre::SceneManager *scene_mgr, Ogre::String suffix, int new_player_id) {
    //clone scene node and level resources
    MovingObject *new_mo = new MovingObject();
    
    new_mo->path_nodes = this->path_nodes;
    new_mo->default_speed = this->default_speed;
    new_mo->default_delay = this->default_delay;
    new_mo->initial_wait = new_mo->waiting_for_ball = this->initial_wait;
   
    try {
        new_mo->body = OgreNewt::cloneBody(this->body, suffix, new_player_id);
    } catch (Ogre::Exception& e) {
        delete new_mo;
        throw e;
    }
    new_mo->ogre_node = (Ogre::SceneNode*)new_mo->body->getOgreNode();
    
    new_mo->ogre_node->setInitialState();
    
    new_mo->body->setUserData(new_mo);
    new_mo->body->setAutoFreeze(0);
    
    new_mo->up_joint = new OgreNewt::BasicJoints::UpVector(new_mo->body->getWorld(), new_mo->body, Ogre::Vector3::UNIT_Y);
    new_mo->movement_joint = NULL;
    new_mo->init();
    
    return new_mo;
}
